
kalman_state kalman_init(float q, float r, float p, float intial_value)
{
  kalman_state result;
  result.q = q;
  result.r = r;
  result.p = p;
  result.x = intial_value;
 
  return result;
}


float kalman_update(kalman_state* state, float measurement)
{
  //prediction update
  //omit x = x
  state->p = state->p + state->q;
 
  //measurement update
  state->k = state->p / (state->p + state->r);
  state->x = state->x + state->k * (measurement - state->x);
  state->p = (1 - state->k) * state->p;
  return state->x;
}


/**************************************************/
//Linear Kalman filter to approximate image gyro data.
/*
//initial values for the kalman filter
float x_est_last = 0;
float P_last = 0;

//the noise in the system
float Q = 0;
float R = 0;

float K;
float P;
float P_temp;
float x_temp_est;
float x_est;
float z_measured; //measured data
float z_real = 0.5; //the ideal value we wish to measure


float Kalman(float input,unsigned short i){


  //initialize with a measurement
  //x_est_last = z_real + frand()*0.09;
  //float sum_error_kalman = 0;
  //float sum_error_measure = 0;

     //do a prediction
    x_temp_est = x_est_last;
    P_temp = P_last + Q;
    //calculate the Kalman gain
    K = P_temp * (1.0/(P_temp + R));
    //measure
    z_measured = z_real + frand()*0.09; //the real measurement plus noise
    //correct
    x_est = x_temp_est + K * (z_measured - x_temp_est); 
    P = (1- K) * P_temp;
    //we have our new system

    printf("Ideal    position: %6.3f \n",z_real);
    printf("Mesaured position: %6.3f [diff:%.3f]\n",z_measured,fabs(z_real-z_measured));
    printf("Kalman   position: %6.3f [diff:%.3f]\n",x_est,fabs(z_real - x_est));

    // sum_error_kalman += fabs(z_real - x_est);
    //sum_error_measure += fabs(z_real-z_measured);

    //update our last's
    P_last = P;
    x_est_last = x_est;
  }



}

http://snippets.dzone.com/posts/show/112158
*/



